#pragma once
#include <srrg_config/configurable.h>
#include <srrg_data_structures/correspondence.h>
#include <srrg_data_structures/platform.h>
#include <srrg_geometry/geometry_defs.h>
#include <srrg_messages/messages/base_sensor_message.h>
#include <srrg_property/property_container.h>
#include <srrg_viewer/drawable_base.h>

namespace srrg2_slam_interfaces {

  /**
   * @brief correspondence finders base class
   * you need to assign it a correspondence vector where to write the result before using it
   */
  class CorrespondenceFinderBase : public srrg2_core::Configurable,
                                   public srrg2_core::PlatformUser,
                                   public srrg2_core::DrawableBase {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CorrespondenceFinderBase() = default;

    //! key(s) for auxiliary data that can be produced/processed by subtypes
    //! @brief - correspondences generated by the aligner unit
    constexpr static const char* auxiliary_data_suffix_correspondences = "_correspondences";
    //! @brief - correspondences generated by loop closure detection unit
    constexpr static const char* auxiliary_data_suffix_closure = "_loop_closure";

    /**
     * @brief correspondences getter
     * @return pointer to the correspondence vector
     */
    const srrg2_core::CorrespondenceVector* correspondences() const {
      return _correspondences;
    }

    /**
     * @brief set the output correspondence vector
     * @param[in] correspondence vector
     */
    void setCorrespondences(srrg2_core::CorrespondenceVector* correspondences_) {
      _correspondences = correspondences_;
    }

    /**
     * @brief provide the raw message data to the module
     * @param[in] raw data message ptr
     */
    void setRawData(srrg2_core::BaseSensorMessagePtr message_) {
      _message = message_;
    }

    /**
     * @brief compute the correspondences
     */
    virtual void compute() = 0;

  protected:
    srrg2_core::CorrespondenceVector* _correspondences = nullptr; /**< correspondence vector*/
    srrg2_core::BaseSensorMessagePtr _message          = nullptr; /**< the message*/
  };

  using CorrespondenceFinderBasePtr = std::shared_ptr<CorrespondenceFinderBase>;

  // typed correspondence finder
  template <typename EstimateType_, typename FixedType_, typename MovingType_>
  class CorrespondenceFinder_ : public CorrespondenceFinderBase {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    using EstimateType = EstimateType_;
    using FixedType    = FixedType_;
    using MovingType   = MovingType_;

    virtual ~CorrespondenceFinder_() = default;

    /**
     * @brief set fixed slice
     * @param[in] fixed_: slice
     */
    void setFixed(FixedType* fixed_) {
      _fixed              = fixed_;
      _fixed_changed_flag = true;
    }
    /**
     * @brief set moving slice
     * @param[in] moving_: slice
     */
    void setMoving(MovingType* moving_) {
      _moving              = moving_;
      _moving_changed_flag = true;
    }

    /**
     * @brief retrieve fixed slice
     * @return pointer to casted fixed data slice
     */
    FixedType* fixed() {
      return _fixed;
    }
    /**
     * @brief retrieve moving slice
     * @return pointer to casted moving data slice
     */
    MovingType* moving() {
      return _moving;
    }
    /**
     * @brief set the pose of the local map in sensor frame
     * @param[in] local map wrt sensor frame
     */
    virtual void setLocalMapInSensor(const EstimateType& local_map_in_sensor_) {
      _local_map_in_sensor              = local_map_in_sensor_;
      _local_map_in_sensor_changed_flag = true;
    }

  protected:
    bool _fixed_changed_flag  = true;    /**< fixed change control flag*/
    bool _moving_changed_flag = true;    /**< moving change control flag*/
    FixedType* _fixed         = nullptr; /**< fixed slice*/
    MovingType* _moving       = nullptr; /**< moving slice*/

    EstimateType _local_map_in_sensor =
      EstimateType::Identity();                     /**< moving in fixed plus sensor pose*/
    bool _local_map_in_sensor_changed_flag = false; /**< transform change control flag*/
  };

} // namespace srrg2_slam_interfaces
